//
// Created by jg on 2021/1/2.
//


#include "env_edt/plan_map.h"

void MAP_EDT::init(ros::NodeHandle &nh) {
    has_map_       = false;
    has_local_map_ = false;

    n_ = nh;
    nh.param("map/path",         file_name_,
             std::string("/home/jg/catkin_ws/btraj/wmr_bezier_planner/map/information_department_index.map"));
    nh.param("map/resolution",   resolution_,   0.2);
    nh.param("map/margin",       map_margin_,   0.5);
    nh.param("map/inflate",      inflate_size_, 0.4);
    nh.param("map/is_mapping",   is_mapping_,   false);

    nh.param("map/x_local_size", x_local_size_, 10.0);
    nh.param("map/y_local_size", y_local_size_, 10.0);

    nh.param("planning/max_vel",       MAX_VEL_,  1.0);

    inv_resolution_ = 1.0 / resolution_;

    collision_map_       = std::make_shared<sdf_tools::CollisionMapGrid>();
    inflate_map_         = std::make_shared<sdf_tools::CollisionMapGrid>();
    collision_map_local_ = std::make_shared<sdf_tools::CollisionMapGrid>();
    loadFile();
}

void MAP_EDT::loadFile() {
    fp_.open(file_name_);
    if(!fp_){
        std::cerr << "open the map file is error ! " << std::endl;
        exit(-1);
    }

    free_cell_ = sdf_tools::COLLISION_CELL(-127);
    obst_cell_ = sdf_tools::COLLISION_CELL( 128);

    builtGlobalMap();
    fp_.close();
}

void MAP_EDT::builtGlobalMap() {
    vector<pair<int, int>> coord;
    std::string line;
    double xmin = 0, xmax = 0, ymin = 0, ymax = 0;
    int xtmp, ytmp;
    bool is_file_empty = true;
    file_lock_.lock();
    while (getline(fp_, line)){
        if(is_file_empty) is_file_empty = false;

        if(line[0] == '$'){

            int index = line.find(',');
            if(line.substr(0, index) == "$obstacle"){
                auto s1 = line.substr(index+1, line.size()-index-1);
                index = s1.find(',');

                xtmp  = atoi(s1.substr(0, index).c_str());

                auto s2 = s1.substr(index+1, s1.size()-index-1);
                index  = s2.find(',');
                ytmp   = atoi(s2.substr(0, index).c_str());

                coord.emplace_back(std::make_pair(xtmp, ytmp));
            }
            else if(line.substr(0, index) == "$resolution") {
                resolution_ = atof(line.substr(index+1, line.size()-index-1).c_str());
            }
            else if(line.substr(0, index) == "$mapsize") {
                line = line.substr(index+1, line.size()-index-1);
                index = line.find(',');
                xmin  = atof(line.substr(0, index).c_str());

                line = line.substr(index+1, line.size()-index-1);
                index  = line.find(',');
                xmax   = atof(line.substr(0, index).c_str());

                line = line.substr(index+1, line.size()-index-1);
                index  = line.find(',');
                ymin   = atof(line.substr(0, index).c_str());

                ymax   = atof(line.substr(index+1, line.size()-index-1).c_str());
            }
            else if(line.substr(0, index) == "$original_point"){
                line = line.substr(index+1, line.size()-index-1);
                index = line.find(',');

                double lat = atof(line.substr(0, index).c_str());       //deg
                line = line.substr(index+1, line.size()-index-1);
                index = line.find(',');

                double lon = atof(line.substr(0, index).c_str());       //deg
                line = line.substr(index+1, line.size()-index-1);

                double alt = atof(line.c_str());                                 //m
                origin_BLH_ << lat*D2R, lon*D2R, alt;                        //rad, rad, m
            }

        }
    }
    file_lock_.unlock();

    if(!is_file_empty) {
        //map size
        if(xmax - xmin < 20) { xmax += 10; xmin -= 10;}
        if(ymax - ymin < 20) { ymax += 10; ymin -= 10;}

        double xsize = xmax - xmin + 2 * map_margin_;
        std::cout << "xmax : " << xmax << std::endl;
        double ysize = ymax - ymin + 2 * map_margin_;
        std::cout << "ymax : " << ymax << std::endl;
        double zsize = resolution_;

        map_origin_ << xmin - map_margin_, ymin - map_margin_, 0;

        Eigen::Translation3d origin_translation(map_origin_(0), map_origin_(1), map_origin_(2));
        Eigen::Quaterniond origin_rotation(1.0, 0.0, 0.0, 0.0);
        Eigen::Affine3d origin_transform = origin_translation * origin_rotation;
        collision_map_.reset(new sdf_tools::CollisionMapGrid(origin_transform, "world", resolution_, xsize, ysize, zsize, free_cell_));

        int64_t index_x, index_y, index_z;
        for (size_t i = 0; i < coord.size(); ++i) {
            index_x = coord[i].first;
            index_y = coord[i].second;
            index_z = 0;
            collision_map_->Set(index_x, index_y, index_z, obst_cell_);
        }

        x_min_ = xmin - map_margin_;
        x_max_ = xmax + map_margin_;
        y_min_ = ymin - map_margin_;
        y_max_ = ymax + map_margin_;
    }
    else { // the file has no data !
        std::cout << "[map generator] : the file " << file_name_ << " has no map data !" << std::endl;
        double xsize = 100 + 2 * map_margin_;
        double ysize = 100 + 2 * map_margin_;
        double zsize = resolution_;
        map_origin_ << -xsize/2, -ysize/2, 0;

        Eigen::Translation3d origin_translation(map_origin_(0), map_origin_(1), map_origin_(2));
        Eigen::Quaterniond origin_rotation(1.0, 0.0, 0.0, 0.0);
        Eigen::Affine3d origin_transform = origin_translation * origin_rotation;
        collision_map_.reset(new sdf_tools::CollisionMapGrid(origin_transform, "world", resolution_, xsize, ysize, zsize, free_cell_));

        x_min_ = -xsize/2;
        x_max_ =  xsize/2;
        y_min_ = -ysize/2;
        y_max_ =  ysize/2;
    }

    has_map_ = true;
    pointInflate();

    vector<double> map_size;
    this->getMapSize(map_size);
    corridor_2D_ = std::make_shared<CORRIDOR_GENERATOR>();
    corridor_2D_->init(inflate_map_, map_size, n_);
}

void MAP_EDT::reMappingGMap(Eigen::Vector3d pt){
    //write collision_map_'s data to file
    writeFile();

    if(!has_map_ || !is_mapping_) return;

    fp_.open(file_name_);
    const double map_size_inflate = 10;
    bool xmin_change = false, ymin_change = false;
    if(pt(0) < x_min_) {
        xmin_change = true;
        x_min_ -= map_size_inflate;
    }
    if(pt(1) < y_min_) {
        ymin_change = true;
        y_min_ -= map_size_inflate;
    }
    x_max_ = (x_max_ > pt(0)) ? x_max_ : (pt(0) + map_size_inflate);
    y_max_ = (y_max_ > pt(1)) ? y_max_ : (pt(1) + map_size_inflate);

    double xsize = (x_max_ - x_min_);
    double ysize = (y_max_ - y_min_);
    double zsize = resolution_;
    map_origin_ << x_min_, y_min_, 0;

    Eigen::Translation3d origin_translation(map_origin_(0), map_origin_(1), map_origin_(2));
    Eigen::Quaterniond origin_rotation(1.0, 0.0, 0.0, 0.0);
    Eigen::Affine3d origin_transform = origin_translation * origin_rotation;
    file_lock_.lock();
    collision_map_.reset(new sdf_tools::CollisionMapGrid(origin_transform, "world", resolution_, xsize, ysize, zsize, free_cell_));

    sdf_tools::COLLISION_CELL odds;
    std::cout << "[map generator] : re mapping ! " << std::endl;
    std::string line;
    int xtmp = 0, ytmp = 0;
    double oddtmp = 0;
    int64_t index_x, index_y, index_z;
    while (getline(fp_, line)){

        if(line[0] == '$'){
            int index = line.find(',');
            if(line.substr(0, index) == "$obstacle"){

                auto s1 = line.substr(index + 1, line.size() - index - 1);
                index = s1.find(',');

                xtmp = atoi(s1.substr(0, index).c_str());

                auto s2 = s1.substr(index + 1, s1.size() - index - 1);
                index = s2.find(',');
                ytmp = atoi(s2.substr(0, index).c_str());

                oddtmp = atof(s2.substr(index+1, s2.size()-index-1).c_str());

                index_x = xtmp + (xmin_change ? (map_size_inflate * inv_resolution_) : 0);
                index_y = ytmp + (ymin_change ? (map_size_inflate * inv_resolution_) : 0);
                index_z = 0;
                odds.occupancy = (float)oddtmp;
                collision_map_->Set(index_x, index_y, index_z, odds);
            }
        }
    }
    file_lock_.unlock();

    fp_.close();
//    pointInflate();
}

void MAP_EDT::getMapSize(vector<double>& map_size) {
    if(!has_map_) return;

    map_size.clear();
    map_size.push_back(x_min_);
    map_size.push_back(x_max_);
    map_size.push_back(y_min_);
    map_size.push_back(y_max_);
}
/* get original point in lat lon alt -rad rad m */
void MAP_EDT::getOrigBLH(Eigen::Vector3d& original) {
    original = origin_BLH_;
}

/*占据栅格膨胀处理*/
void MAP_EDT::pointInflate() {
    int64_t n_inflate = ceil(inflate_size_ * inv_resolution_);
    vector<pair<int64_t , int64_t >> v_obs;

    auto isLocalInside = [&](const Eigen::Vector3d& loc){
        const auto& index = collision_map_local_->LocationToGridIndex(loc);
        return collision_map_local_->Inside(index);
    };

    for (int64_t x = 0; x < collision_map_->GetNumXCells(); x++)
        for (int64_t y = 0; y < collision_map_->GetNumYCells(); y++){
            const auto& loc = collision_map_->GridIndexToLocation(Eigen::Vector3i(x, y, 0));
            if(!is_mapping_ && has_local_map_ && isLocalInside(loc)){
                const auto& local_cell = collision_map_local_->Get(loc[0], loc[1], loc[2]);
                if(local_cell.second && local_cell.first.occupancy > 0.5)
                    v_obs.emplace_back(std::make_pair(x, y));
            } else {
                const auto& global_cell = collision_map_->Get(x, y, 0);
                if(global_cell.first.occupancy > 0.5)
                    v_obs.emplace_back(std::make_pair(x, y));
            }
        }
    //mapping inflate map
    map_lock_.lock();
    double xsize = (x_max_ - x_min_);
    double ysize = (y_max_ - y_min_);
    double zsize = resolution_;
    Eigen::Translation3d origin_translation(map_origin_(0), map_origin_(1), map_origin_(2));
    Eigen::Quaterniond origin_rotation(1.0, 0.0, 0.0, 0.0);
    Eigen::Affine3d origin_transform = origin_translation * origin_rotation;
    inflate_map_.reset(new sdf_tools::CollisionMapGrid(origin_transform, "world", resolution_, xsize, ysize, zsize, free_cell_));

    auto iter = v_obs.begin();
    for (; iter != v_obs.end(); ++iter) {
        auto point = *iter;
        for (int64_t nx = -n_inflate; nx < n_inflate; ++nx)
            for (int64_t ny = -n_inflate; ny < n_inflate; ++ny)
                if(pow(nx, 2) + pow(ny, 2) <= pow(n_inflate, 2))
                    inflate_map_->Set(point.first + nx, point.second + ny, 0, obst_cell_);
    }

    map_lock_.unlock();
}

vector<int64_t> MAP_EDT::location2Index(const Eigen::Vector3d& location, PLANSTYLE style) {
    vector<int64_t> grid_index;

    Eigen::Vector3d pt = location;
    double& z = pt[2];
    z = min(0.2 - 1e-2, max(1e-2, z));

    if (style == GLOBAL){
        if(!has_map_)
            return grid_index;
        grid_index = collision_map_->LocationToGridIndex3d(pt);
    }

    return grid_index;
}

void MAP_EDT::GlobalIndex2Location(const vector<int64_t> &index, Eigen::Vector3d &pt) {
    if(!has_map_) return;

    auto pos = inflate_map_->GridIndexToLocation(index[0], index[1], index[2]);
    pt << pos[0], pos[1], pos[2];
}

bool MAP_EDT::getGlobalMap(Map_Ptr& _map) {
    if(!has_map_) return false;

    _map = this->inflate_map_;
//    _map = this->collision_map_;
    return true;
}

bool MAP_EDT::getLocalMap(Map_Ptr& _map) {
    if(!has_map_) return false;

    _map = this->collision_map_local_;
    return true;
}

/******  构建局部地图  ******/
void MAP_EDT::builtLocalMap(const Eigen::Vector3d& start_pt) {
    if(start_pt[0] < x_min_ || start_pt[0] > x_max_
       || start_pt[1] < y_min_ || start_pt[1] > y_max_){
        has_local_map_ = false;
        return;
    }

    robot_pt_ = start_pt;

    shared_ptr<sdf_tools::CollisionMapGrid> local_base_map;
    // 根据局部地图是否影响全局地图 选择局部地图的基础
    // 局部影响全局 则局部地图在 inflate_map_ 上进行更新
    local_base_map = is_mapping_ ? inflate_map_ : collision_map_;

    auto index_in_map  = this->collision_map_->LocationToGridIndex(robot_pt_);
    auto pt_from_index = this->collision_map_->GridIndexToLocation(index_in_map);
    double local_c_x = pt_from_index(0) - x_local_size_/2.0;
    double local_c_y = pt_from_index(1) - y_local_size_/2.0;

    local_origin_ << local_c_x, local_c_y, 0;

    Eigen::Translation3d origin_local_translation( local_origin_(0), local_origin_(1), local_origin_(2));
    Eigen::Quaterniond origin_local_rotation(1.0, 0.0, 0.0, 0.0);
    Eigen::Affine3d origin_local_transform = origin_local_translation * origin_local_rotation;

    double z_local_size = resolution_;
    map_lock_.lock();
    collision_map_local_.reset(new sdf_tools::CollisionMapGrid(origin_local_transform, "world", resolution_, x_local_size_, y_local_size_, z_local_size, free_cell_));

    auto point_local_begin = collision_map_local_->GridIndexToLocation(0, 0, 0);
    double &px = point_local_begin[0];
    double &py = point_local_begin[1];
    double &pz = point_local_begin[2];
    const auto& xsize = collision_map_local_->GetNumXCells();
    const auto& ysize = collision_map_local_->GetNumYCells();
    const auto& zsize = collision_map_local_->GetNumZCells();
    if(px <= this->x_min_ || py <= this->y_min_){
        auto gxmin = px <= x_min_ ? x_min_ : px;
        auto gymin = py <= y_min_ ? y_min_ : py;
        auto index_local_start = collision_map_local_->LocationToGridIndex(gxmin, gymin, 0);
        const auto& lx = index_local_start[0];
        const auto& ly = index_local_start[1];
        const auto& lz = index_local_start[2];
        auto index_global = collision_map_->LocationToGridIndex(gxmin, gymin, 0);
        const int64_t begin_x = index_global[0] - lx;
        const int64_t begin_y = index_global[1] - ly;
        for(int64_t x = lx; x < xsize; x++)
            for(int64_t y = ly; y < ysize; y++)
                for(int64_t z = lz; z < zsize; z++){
                    auto cell = local_base_map->Get(x+begin_x, y+begin_y, z);
                    if(cell.second && cell.first.occupancy > 0.5){
                        collision_map_local_->Set(x, y, z, obst_cell_);
                    }
                }
    } else {
        auto point_map_global  = collision_map_->LocationToGridIndex(px, py, pz);
        const auto& gx = point_map_global[0];
        const auto& gy = point_map_global[1];
        const auto& gz = point_map_global[2];
        for(int64_t x = 0; x < xsize; x++)
            for(int64_t y = 0; y < ysize; y++)
                for(int64_t z = 0; z < zsize; z++){
                    auto cell = local_base_map->Get(gx+x, gy+y, gz+z);
                    if(cell.second && cell.first.occupancy > 0.5){
                        collision_map_local_->Set(x, y, z, obst_cell_);
                    }
                }
    }

    has_local_map_ = true;
    map_lock_.unlock();


    static bool is_first = true;
    if(is_mapping_  || is_first){
        //if 语句防止检测不到临时障碍物
        float oob_value = INFINITY;
        esdf_ = collision_map_local_->ExtractDistanceField(oob_value);

        is_first = false;
    }
}

void MAP_EDT::getMapOrigin(Eigen::Vector3d& _origin) {
    if (!has_map_) {
        std::cerr << "[map generator :] get map origin false !" << std::endl;
        exit(-1);
    }

    _origin = this->map_origin_;
}

void MAP_EDT::getLocalOrigin(Eigen::Vector3d& _origin){
    if (!has_map_) {
        std::cerr << "[map generator :] get map origin false !" << std::endl;
        exit(-1);
    }

    _origin = this->local_origin_;
}

//(_x, _y)为机器人坐标
void MAP_EDT::refreshMap(double _x, double _y, vector<pair<Eigen::Vector3d, bool >>& _vec_point) {
    auto is_equal = [](vector<int64_t>& a, vector<int64_t>& b)->bool {
        if(a.size() != 3 || b.size() != 3) return false;
        for (int i = 0; i < 3; ++i) {
            if(a[i] != b[i]) return false;
        }
        return true;
    };

    if(_x < x_min_ || _x > x_max_ || _y < y_min_ || _y > y_max_) return;
    /*根据is_mapping选择地图*/
    /*ture  --修改全局地图，改变部分保留进地图文件*/
    /*false --仅修改局部地图，不影响全局地图*/
    shared_ptr<sdf_tools::CollisionMapGrid> _map;
    _map = is_mapping_ ? collision_map_ : collision_map_local_;
    /*机器人作为在地图上的索引，用于刷新机器与障碍物之间栅格的概率*/
    auto index_rob = _map->LocationToGridIndex(_x, _y, 0.1);
    vec_pt_.swap(_vec_point);
    /*存储转换为栅格的点云，避免多个点云在同个栅格内造成重复计算*/
    vector<vector<int64_t>> idxs_ground, idxs_obs;
    auto iter = vec_pt_.begin();
    sdf_tools::COLLISION_CELL odds;
    map_lock_.lock();
    for(; iter != vec_pt_.end(); ++iter){
        auto& pt = iter->first;
        auto cell = _map->Get(pt[0], pt[1], pt[2]);
        if(!iter->second){ //处理地面信息
//            if(!is_mapping_) continue;
            /*检查点云占据的栅格是否已被处理过*/
            auto index_ground = getGroundIndex(cell.second, pt, _x ,_y);
            bool is_jump = false;
            for(auto idx : idxs_ground){
                if(is_equal(idx, index_ground)){
                    is_jump = true;
                    break;
                }
            }
            if(is_jump) continue;
            idxs_ground.push_back(index_ground);

            odds.occupancy = oddsmin;
            if(!is_mapping_)
                _map->Set(index_ground[0], index_ground[1], index_ground[2], odds);
            bresenHamLine(_map, index_rob[0], index_rob[1], index_ground[0], index_ground[1]);
        }
        else if(cell.second){ //处理障碍物信息
            if(abs(pt[0] - _x) > 0.5*x_local_size_ || abs(pt[1] - _y) > 0.5*x_local_size_)
                continue;
            //不需要检查占据栅格是否被重复处理
            //get the occupied state of cell : log p(o)/p(f)
            float& value = cell.first.occupancy;

            if(is_mapping_){
                value += looccu;
                value  = max(oddsmin, min(value, oddsmax));
            } else {
                value = oddsmax;
            }

            odds.occupancy = value;
            auto index_obs = _map->LocationToGridIndex(pt[0], pt[1], 0.1);
            _map->Set(index_obs[0], index_obs[1], index_obs[2], odds);
//            if(is_mapping_) //记录地图或无地面点云信息时启用
                bresenHamLine(_map, index_rob[0], index_rob[1], index_obs[0], index_obs[1]);
        }
        else {
            //探测数据超出collision_map_范围
            auto outside_map = [&](){
                return !collision_map_->Get3d(pt).second;
            };

            if(is_mapping_ && outside_map()) {
                reMappingGMap(pt);
                break;
            }
        }
    }
//    _map->Set(index_rob[0], index_rob[1], index_rob[2], free_cell_);
    map_lock_.unlock();

    pointInflate();

    float oob_value = INFINITY;
    esdf_ = collision_map_local_->ExtractDistanceField(oob_value);

    //刷新corridor的地图
    vector<double> map_size;
    this->getMapSize(map_size);
    corridor_2D_->refreshMap(inflate_map_, map_size);
}
//mark -true 表示为障碍物点云 -false 表示为地面点云
//地面点云超出局部地图边界时，选择机器人中心到点云连线与局部地图边界的交点
//返回地图索引值
vector<int64_t> MAP_EDT::getGroundIndex(bool mark, Eigen::Vector3d &pt, const double& _x, const double& _y) {
    shared_ptr<sdf_tools::CollisionMapGrid> _map;
    _map = is_mapping_ ? collision_map_ : collision_map_local_;

    if(mark) {
        return _map->LocationToGridIndex(pt[0], pt[1], 0.1);
    }

    if(pt[0] == _x){
        pt[1] = pt[1] > _y ? (_y + 0.5*x_local_size_ - 0.5*resolution_) : (_y - 0.5*x_local_size_ + 0.5*resolution_);
    } else {
        double gradient = (pt[1] - _y) / (pt[0] - _x);
        if(gradient >= -1 && gradient <= 1){
            double _k = (0.5*x_local_size_ - 0.5*resolution_) / abs(pt[0] - _x);
            pt[0] = pt[0] > _x ? (_x + 0.5*x_local_size_ - 0.5*resolution_) : (_x - 0.5*x_local_size_ + 0.5*resolution_);
            pt[1] = pt[1] > _y ? (_y + _k * abs(pt[1] - _y)) : (_y - _k * abs(pt[1] - _y));
        } else {
            double _k = (0.5*x_local_size_ - 0.5*resolution_) / abs(pt[1] - _y);
            pt[1] = pt[1] > _y ? (_y + 0.5*x_local_size_ - 0.5*resolution_) : (_y - 0.5*x_local_size_ + 0.5*resolution_);
            pt[0] = pt[0] > _x ? (_x + _k * abs(pt[0] - _x)) : (_x - _k * abs(pt[0] - _x));
        }
    }

    return _map->LocationToGridIndex(pt[0], pt[1], 0.1);
}

void MAP_EDT::bresenHamLine(shared_ptr<sdf_tools::CollisionMapGrid>& _map, int64_t x0, int64_t y0, int64_t& x1, int64_t& y1) {
    int64_t dx = abs(x1 - x0), sx = x0 < x1 ? 1 : -1;
    int64_t dy = abs(y1 - y0), sy = y0 < y1 ? 1 : -1;
    int64_t err = (dx > dy? dx : -dy)/2, e2;

    sdf_tools::COLLISION_CELL odds;
    for(;;){
        if(x0==x1 && y0 == y1) break;
        //
        auto cell = _map->Get((long)x0, (long)y0, 0);
        float value = cell.first.occupancy + lofree;

        value = (value > oddsmax) ? oddsmax : value;
        value = (value < oddsmin) ? oddsmin : value;
        odds.occupancy = is_mapping_ ? value : oddsmin;

        _map->Set(x0, y0, 0, odds);

        e2 = err;
        if(e2 > -dx) { err -= dy; x0 += sx;}
        if(e2 <  dy) { err += dx; y0 += sy;}
    }
}

void MAP_EDT::writeFile() {
    file_lock_.lock();
    wf_.open(file_name_);
    if(!wf_){
        std::cerr << "open the map file is error ! " << std::endl;
        exit(-1);
    }

    wf_ << "$resolution," << resolution_ << endl;
    wf_ << "$original_point,30.528628340918,114.35572874791,22.813257" << endl;
    wf_ << "$mapsize," << x_min_ + map_margin_ << ","
        << x_max_ - map_margin_ << ","
        << y_min_ + map_margin_ << ","
        << y_max_ - map_margin_ << std::endl;
    for (int64_t x = 0; x < collision_map_->GetNumXCells(); x++)
        for (int64_t y = 0; y < collision_map_->GetNumYCells(); y++){
            auto cell = collision_map_->Get(x, y, 0);
            if (cell.first.occupancy > 0.5) {
                wf_ << "$obstacle," << x << "," << y
                    << "," << cell.first.occupancy << std::endl;
                auto pos = collision_map_->GridIndexToLocation(x, y, 0);
                wf_ << "$real_xy," << pos[0] << "," << pos[1] << "," << pos[2] << std::endl;
            }
        }

    wf_.close();
    file_lock_.unlock();

    std::cout << "rewrite map file ! " << std::endl;
}

bool MAP_EDT::checkCoordObs(Eigen::Vector3d checkPt)
{
    if(inflate_map_->Get(checkPt(0), checkPt(1), checkPt(2)).first.occupancy > 0.0 )
        return true;

    return false;
}

void MAP_EDT::getFMMGrid(FMGrid3D& grid_fmm, double& max_vel) {

    const unsigned int size_x = inflate_map_->GetNumXCells();
    const unsigned int size_y = inflate_map_->GetNumYCells();
    const unsigned int size_z = inflate_map_->GetNumZCells();
    Coord3D dimsize {size_x, size_y, size_z};
    grid_fmm.resize(dimsize);

    unsigned int idx;
    double flow_vel;
    max_vel = MAX_VEL_ * 0.7;
    vector<unsigned int> obs;
    Eigen::Vector3d pt;
    for(unsigned int k = 0; k < size_z; k++)
        for(unsigned int j = 0; j < size_y; j++)
            for(unsigned int i = 0; i < size_x; i++){
                //grid_fmm's grid index : idx
                idx = k * size_y * size_x + j * size_x + i;
                pt << (i + 0.5) * resolution_ + map_origin_(0),
                        (j + 0.5) * resolution_ + map_origin_(1),
                        (k + 0.5) * resolution_ + map_origin_(2);

                Eigen::Vector3i index   = collision_map_local_->LocationToGridIndex(pt);
                Eigen::Vector3i g_index = inflate_map_->LocationToGridIndex(pt);

                if(collision_map_local_->Inside(index))
                {
                    double d = sqrt(esdf_.GetImmutable(index).first.distance_square) * resolution_;
                    d = (d == INFINITY ? 2 : d);
                    flow_vel = velMapping(pt, d, max_vel);
                }
                else if(inflate_map_->Inside(g_index)){
                    int64_t ix = g_index[0];
                    int64_t iy = g_index[1];
                    int64_t iz = g_index[2];
                    auto cell = inflate_map_->Get(ix, iy, iz);
                    if(cell.first.occupancy > 0.5) {
                        flow_vel = 0;
                    } else {
                        flow_vel = max_vel;
                    }
                }
                else
                    flow_vel = max_vel;

                if( j == 0 || j == (size_y - 1) || i == 0 || i == (size_x - 1) )
                    flow_vel = 0.0;

                grid_fmm[idx].setOccupancy(flow_vel);
                if (grid_fmm[idx].isOccupied())
                    obs.push_back(idx);
            }

    grid_fmm.setOccupiedCells(std::move(obs));
    grid_fmm.setLeafSize(resolution_);
}

double MAP_EDT::velMapping(const Eigen::Vector3d& pt, double d, double max_v){
    double vel;
    const double len_map = 1.5;

    const auto get_vel = [&](double tmp_dis, double len){
        double invlen = 1 / len;
        if( tmp_dis <= 0.25 * len)
            return 2.0 * tmp_dis * invlen * tmp_dis * invlen;
        else if(tmp_dis > 0.25 * len && tmp_dis <= 0.75 * len)
            return 1.5 * tmp_dis * invlen - 0.25;
        else if(tmp_dis > 0.75 * len && tmp_dis <= 1.0 * len)
            return - 2.0 * (tmp_dis * invlen - 1.0) * (tmp_dis * invlen - 1.0) + 1;
        else
            return 1.0;
    };

    vel = get_vel(d, len_map);

    return vel * max_v;
}

pair<vector<unsigned int>, unsigned int> MAP_EDT::FMMSet2Point
(FMGrid3D& grid_fmm, Eigen::Vector3d start_pt, Eigen::Vector3d end_pt){
    Eigen::Vector3d startIdx3d = (start_pt - map_origin_) * inv_resolution_;
    Eigen::Vector3d endIdx3d   = (end_pt   - map_origin_) * inv_resolution_;

    Coord3D goal_point = {(unsigned int)startIdx3d[0], (unsigned int)startIdx3d[1], (unsigned int)startIdx3d[2]};
    Coord3D init_point = {(unsigned int)endIdx3d[0],   (unsigned int)endIdx3d[1],   (unsigned int)endIdx3d[2]};

    unsigned int startIdx;
    vector<unsigned int> startIndices;
    grid_fmm.coord2idx(init_point, startIdx);
    startIndices.push_back(startIdx);

    unsigned int goalIdx;
    grid_fmm.coord2idx(goal_point, goalIdx);

    return make_pair(startIndices, goalIdx);
}

void MAP_EDT::FMMPath3D2vecPath(Path3D& path3D, vector<Eigen::Vector3d>& path_coord){
    double coord_x, coord_y, coord_z;
    for( int i = 0; i < (int)path3D.size(); i++){
        coord_x = max(min( (path3D[i][0]+0.5) * resolution_ + map_origin_[0], x_max_), x_min_);
        coord_y = max(min( (path3D[i][1]+0.5) * resolution_ + map_origin_[1], y_max_), y_min_);
        coord_z = 0;

        Eigen::Vector3d pt(coord_x, coord_y, coord_z);
        path_coord.push_back(pt);
    }
}

void MAP_EDT::getCorridor(vector<Cube> &corridor, vector<Eigen::Vector3d> &path, vector<double> &time) {
    vector<Cube>().swap(corridor);

    corridor_2D_->corridorGeneration(corridor, path, time);
}

void MAP_EDT::getDistance(double &dis, Eigen::Vector3d& pt) {
    pt[2] = min(max(pt[2], 1e-2), 0.2-1e-2);

//    map_lock_.lock();
    Eigen::Vector3i index = collision_map_local_->LocationToGridIndex(pt);
    dis = sqrt(esdf_.GetImmutable(index).first.distance_square) * resolution_;
//    map_lock_.unlock();
}

void MAP_EDT::getDistance(double &dis, vector<int64_t>& global_index) {
//    map_lock_.lock();
    auto gpos = inflate_map_->GridIndexToLocation(global_index[0], global_index[1], global_index[2]);
    Eigen::Vector3d pt(gpos[0], gpos[1], gpos[2]);
    auto local_idx = collision_map_local_->LocationToGridIndex(pt);
    dis = sqrt(esdf_.GetImmutable(local_idx).first.distance_square) * resolution_;
//    map_lock_.unlock();
}
/* 获取(全局)位置pos的 距离信息dist(m) 和 梯度信息grad*/
void MAP_EDT::evaluateEDTWithGrad(const Eigen::Vector3d &pos, double &dist, Eigen::Vector3d &grad) {

    Eigen::Vector3d diff;
    Eigen::Vector3d sur_pts[2][2][2];
    this->getSurroundPts(pos, sur_pts, diff);

    double dists[2][2][2]; //距离 -m
    this->getSurroundDistance(sur_pts, dists);

    interpolateTrilinear(dists, diff, dist, grad);
    grad[2] = 0.0;   //二维空间， 将梯度的z轴方向置零
}

void MAP_EDT::interpolateTrilinear(double values[2][2][2], const Eigen::Vector3d &diff, double &value,
                                   Eigen::Vector3d &grad) {
    // trilinear interpolation
    double v00 = (1 - diff(0)) * values[0][0][0] + diff(0) * values[1][0][0];
    double v01 = (1 - diff(0)) * values[0][0][1] + diff(0) * values[1][0][1];
    double v10 = (1 - diff(0)) * values[0][1][0] + diff(0) * values[1][1][0];
    double v11 = (1 - diff(0)) * values[0][1][1] + diff(0) * values[1][1][1];
    double v0 = (1 - diff(1)) * v00 + diff(1) * v10;
    double v1 = (1 - diff(1)) * v01 + diff(1) * v11;

    value = (1 - diff(2)) * v0 + diff(2) * v1;

    grad[2] = (v1 - v0) * inv_resolution_;
    grad[1] = ((1 - diff[2]) * (v10 - v00) + diff[2] * (v11 - v01)) * inv_resolution_;
    grad[0] = (1 - diff[2]) * (1 - diff[1]) * (values[1][0][0] - values[0][0][0]);
    grad[0] += (1 - diff[2]) * diff[1] * (values[1][1][0] - values[0][1][0]);
    grad[0] += diff[2] * (1 - diff[1]) * (values[1][0][1] - values[0][0][1]);
    grad[0] += diff[2] * diff[1] * (values[1][1][1] - values[0][1][1]);
    grad[0] *= inv_resolution_;
}

void MAP_EDT::getSurroundDistance(Eigen::Vector3d pts[2][2][2], double dists[2][2][2]){
    for (int x = 0; x < 2; x++)
        for (int y = 0; y < 2; y++)
            for (int z = 0; z < 2; z++) {
                this->getDistance(dists[x][y][z], pts[x][y][z]);
            }
}

void MAP_EDT::getSurroundPts(const Eigen::Vector3d& pos, Eigen::Vector3d pts[2][2][2], Eigen::Vector3d &diff) {
    if(!checkPOS(pos)) return;

    /* interpolation position */
    Eigen::Vector3d pos_m = pos - 0.5 * resolution_ * Eigen::Vector3d::Ones();
    pos_m[0] = max(pos_m[0], x_min_);
    pos_m[1] = max(pos_m[1], y_min_);
    pos_m[2] = max(pos_m[2], 0.0);

    auto idx     = inflate_map_->LocationToGridIndex(pos_m);
    auto pos_idx = inflate_map_->GridIndexToLocation(idx);

    diff = (pos - pos_idx) * inv_resolution_;

    for (int x = 0; x < 2; x++)
        for (int y = 0; y < 2; y++)
            for (int z = 0; z < 2; z++) {
                Eigen::Vector3i current_idx = idx + Eigen::Vector3i(x, y, z);
                pts[x][y][z] = inflate_map_->GridIndexToLocation(current_idx);
            }

}

void MAP_EDT::setGlobalMapMarker(visualization_msgs::Marker &map_marker) {
    map_marker.points.clear();

    geometry_msgs::Point point;
    auto& _map = inflate_map_;
    map_lock_.lock();
    for (int64_t x = 0; x < _map->GetNumXCells(); ++x)
        for (int64_t y = 0; y < _map->GetNumYCells(); ++y) {
            auto cell = _map->Get(x, y, 0);
            if (cell.first.occupancy > 0.0) {
                auto pt = _map->GridIndexToLocation(x, y, 0);
                point.x = pt[0];
                point.y = pt[1];
                point.z = pt[2];
                map_marker.points.push_back(point);
            }
        }

    visMapBoundery(map_marker);
    map_lock_.unlock();
}

void MAP_EDT::setLocalMapMarker(visualization_msgs::Marker &map_marker) {
    map_marker.points.clear();

    geometry_msgs::Point point;
    auto& _map = collision_map_local_;
    map_lock_.lock();
    for (int64_t x = 0; x < _map->GetNumXCells(); ++x)
        for (int64_t y = 0; y < _map->GetNumYCells(); ++y) {
            auto cell = _map->Get(x, y, 0);
            if (cell.first.occupancy > 0.0) {
                auto pt = _map->GridIndexToLocation(x, y, 0);
                point.x = pt[0];
                point.y = pt[1];
                point.z = pt[2];
                map_marker.points.push_back(point);
            }
        }
    map_lock_.unlock();
//    visMapBoundery(map_marker);
}

void MAP_EDT::visMapBoundery(visualization_msgs::Marker &map_marker) {
    //map boundery
    geometry_msgs::Point p1, p2;
    for(double xmar = x_min_; xmar <= x_max_; xmar += resolution_){
        p1.x = xmar;    p2.x = xmar;
        p1.y = y_min_;  p2.y = y_max_;
        p1.z = 0;       p2.z = 0;
        map_marker.points.push_back(p1);
        map_marker.points.push_back(p2);
    }
    for(double ymar = y_min_; ymar <= y_max_; ymar += resolution_){
        p1.x = x_min_;    p2.x = x_max_;
        p1.y = ymar;      p2.y = ymar;
        p1.z = 0;         p2.z = 0;
        map_marker.points.push_back(p1);
        map_marker.points.push_back(p2);
    }
}
